/**
  ****************************(C) COPYRIGHT 2025 ROBOT-Z****************************
  * @file       chassis_power_control.c/h
  * @brief      2025赛季哨兵底盘功率控制
  * @note       计算超级电容DCDC充放电功率：最大功率限制 - PID能量缓冲环的输出
  *				底盘功率较低：
  *				chassis_power_buffer > 30 缓冲能量环输出 < 0
  *				input_power > 0  电容充电
  *				底盘功率较高：
  *				chassis_power_buffer <30 缓冲能量环输出 > 0
  *				input_power < 0  电容放电
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Nov-11-2019     RM              1. 添加底盘功率限制
  *  V2.0.0		Dec-20-2024		卢俊烨			2. 更换了功率解算
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2025 ROBOT-Z****************************
  */

#include "chassis_power_control.h"
#include "referee.h"
#include "arm_math.h"
#include "pid.h"
#include "chassis_task.h"

uint8_t cap_state = 0;		// 超级电容默认关闭，调试时可以强制打开设置为1
static const fp32 buffer_pid[3] = {BUFFER_PID_KP, BUFFER_PID_KI, BUFFER_PID_KD};
static int32_t power_control_time = 0;

/**
  * @brief          限制功率，主要限制电机电流
  * @param          chassis_power_control: 底盘数据
  * @retval         none
  */
void chassis_power_control(chassis_move_t *chassis_power_control)
{
    PID_init(&chassis_power_control->buffer_pid, PID_POSITION, buffer_pid, 130, 50);	// 缓冲PID初始化

	static uint16_t max_power_limit = 0;		// 电池（裁判系统）最大功率限制
	static fp32 chassis_max_power = 0;			// 底盘电机可用最大功率
    static fp32 input_power = 0;				// 从电池（裁判系统）输入的功率
	static fp32 initial_give_power[4];			// 每个电机的当前功率
	fp32 initial_total_power = 0.0f;			// 底盘四个电机的总功率
	static fp32 scaled_give_power[4];			// 每个电机缩放后的功率

	static fp32 chassis_power = 0.0f;
	static uint16_t chassis_power_buffer = 0;

	const fp64 toque_coefficient = 1.99688994e-6f;	// 交互项(20/16384)*(0.3)*(187/3591)/9.55
	const fp64 k1 = 1.937e-07;						// 二次项k1
	const fp64 k2 = 3.7e-07;							// 二次项k2
	const fp32 constant = 0.8f;						// 常数项

	get_chassis_power_and_buffer(&chassis_power, &chassis_power_buffer);			// 通过裁判系统获取底盘功率数据
	PID_calc(&chassis_power_control->buffer_pid, chassis_power_buffer, 30);	// 期望留存的缓冲能量：30 缓冲能量上限：60
	get_chassis_power_limit(&max_power_limit);										// 获取最大限制功率 根据机器人等级提高而提升上限

	input_power = max_power_limit - chassis_power_control->buffer_pid.out;

	power_control_time++;
	if (power_control_time == 10)
	{
   		CAN_cmd_supercap(input_power);	// 设置超级电容的输入功率
		power_control_time = 0;
	}

	/* 是否开启超级电容 */
	if ((chassis_move.vx_set * chassis_move.vx_set + chassis_move.vy_set * chassis_move.vy_set >= 4.0f) && (chassis_move.chassis_sup->capacitance_percentage > 25))
	{
		cap_state = 1;
	}
    else
    {
        cap_state = 0;
    }

	/* 计算底盘可使用的最大功率 */
	if (chassis_move.chassis_sup->capacitance_percentage > 5)		// 超级电容剩余容量大于5％
	{
		if (cap_state == 0)
		{
			chassis_max_power = input_power + 5;	// 不用超级电容，底盘可用功率略大于输入功率，避免电容器一直充满，提高能源利用率
		}
		else
		{
			chassis_max_power = input_power + 80;	// 使用超级电容，底盘功率狠狠用！
		}
	}
	else	// 超级电容电量不足
	{
		chassis_max_power = input_power;
	}

	/* 正解算 根据底盘电机的测量数据 计算得到当前功率 */
	for (uint8_t i = 0; i < 4; i++)		// 首先获取所有初始电机功率和总电机功率
	{
		initial_give_power[i] = chassis_power_control->motor_speed_pid[i].out * toque_coefficient * chassis_power_control->motor_chassis[i].chassis_motor_measure->speed_rpm +
								k1 * chassis_power_control->motor_chassis[i].chassis_motor_measure->speed_rpm * chassis_power_control->motor_chassis[i].chassis_motor_measure->speed_rpm +
								k2 * chassis_power_control->motor_speed_pid[i].out * chassis_power_control->motor_speed_pid[i].out + constant;

		if (initial_give_power < 0)		// 不包括负功率（暂时性）
			continue;
		initial_total_power += initial_give_power[i];
	}

	/* 逆解算 若当前功率超出最大功率 则计算缩放因子进行功率缩放 */
	if (initial_total_power > chassis_max_power)
	{
		fp32 power_scale = chassis_max_power / initial_total_power;			// 计算比例缩放因子
		for (uint8_t i = 0; i < 4; i++)
		{
			scaled_give_power[i] = initial_give_power[i] * power_scale;		// 获得缩放功率
			if (scaled_give_power[i] < 0)
			{
				continue;
			}

			/* 计算期望功率对应的力矩电流 */
			fp32 b = toque_coefficient * chassis_power_control->motor_chassis[i].chassis_motor_measure->speed_rpm;
			fp32 c = k2 * chassis_power_control->motor_chassis[i].chassis_motor_measure->speed_rpm * chassis_power_control->motor_chassis[i].chassis_motor_measure->speed_rpm - scaled_give_power[i] + constant;

			/* 根据原始力矩的方向选择计算公式 */
			if (chassis_power_control->motor_speed_pid[i].out > 0)
			{
				fp32 temp = (-b + sqrt(b * b - 4 * k1 * c)) / (2 * k1);
				if (temp > 16000)
				{
					chassis_power_control->motor_speed_pid[i].out = 16000;
				}
				else
					chassis_power_control->motor_speed_pid[i].out = temp;
			}
			else
			{
				fp32 temp = (-b - sqrt(b * b - 4 * k1 * c)) / (2 * k1);
				if (temp < -16000)
				{
					chassis_power_control->motor_speed_pid[i].out = -16000;
				}
				else
					chassis_power_control->motor_speed_pid[i].out = temp;
			}
		}
	}
}
